/*
 * hcsr04.c
 *
 *  Created on: May 21, 2021
 *      Author: kirk_
 */

#include "hcsr04.h"
#include "cmsis_os.h"

#include "kd_delay.h"

#include "stdio.h"

void HCSR04_Init(
		HCSR04_t* khcsr,
		GPIO_TypeDef* Tport,
		uint16_t Tpin,
		GPIO_TypeDef* Eport,
		uint16_t Epin
		)
{
	KDL_Pin_Init(&khcsr->Trig, Tport, Tpin);
	KDL_Pin_Init(&khcsr->Echo, Eport, Epin);

	KDL_Pin_Reset(&khcsr->Trig);
}

uint32_t HCSR04_Measure(HCSR04_t * khcsr)
{
	uint32_t start, end;

	/* 10us of pulse */
	KDL_Pin_Set(&khcsr->Trig);
	KD_Delay(10);
	KDL_Pin_Reset(&khcsr->Trig);

	/* Wait for echo high */
	end = osKernelGetSysTimerCount();
	do {
		start = osKernelGetSysTimerCount();
	}
	while(((start - end) < (HCSR04_TimeOut<<2)) && !KDL_Pin_Read(&khcsr->Echo));

	/* Wait for echo low */
	do {
		end = osKernelGetSysTimerCount();
	}
	while(((end - start) < HCSR04_TimeOut) && KDL_Pin_Read(&khcsr->Echo));

	khcsr ->Ticks = end - start;

	return khcsr->Ticks;
}

uint16_t HCSR04_GetDistance(HCSR04_t * khcsr)
{
	double dtmp;

	dtmp = khcsr->Ticks;
	dtmp /= 1000.00;
	dtmp *= 340.00;
	dtmp /= 1000.00;

	return (uint16_t)dtmp;
}
